#include <esp_now.h>
#include <WiFi.h>

//定义电机驱动引脚
const int motorL1 = 9;  //左电机
const int motorL2 = 10;
const int motorR1 = 11;  //右电机
const int motorR2 = 12;
const int motorT1 = 13;  //炮塔电机
const int motorT2 = 14;
const int motorP1 = 15;  //炮管电机
const int motorP2 = 16;

//软件减速比（数值越大电机越慢）
const int ratioRL = 1;  //前后减速比
const int ratioT = 3;  //炮塔减速比
const int ratioP = 5;  //炮管减速比

//LED引脚及呼吸状态相关参数
const int LED_PIN = 38;             //LED引脚
unsigned long previousMillis = 0;   // 记录上次状态切换时间
const long onInterval = 1000;       // LED亮的时间
const long offInterval = 1000;      // LED灭的时间
bool isLedOn = false;               // LED当前状态

// ADC引脚及参数
const int ADC_PIN = 5;               // ADC引脚
const float R1 = 22000.0;        // 分压电阻R1=22kΩ
const float R2 = 10000.0;        // 分压电阻R2=10kΩ
const float REF_VOLTAGE = 3.29;  // ESP32 ADC参考电压（默认3.26，实测更准）
int voltage;                    //电压

//ESPNOW配对相关参数
uint8_t masterMac[6] = {0};                   //主机MAC
bool isPaired = false;                        //配对状态标志
volatile unsigned long lastResponseTime = 0;  // 最后响应时间戳
const unsigned long RESPONSE_TIMEOUT = 1000;  // 连接超时时间1S

// 定义配对消息
#define PAIRING_REQUEST "PAIRING_REQ"
#define PAIRING_RESPONSE "PAIRING_RESP"

// ESPNOW 控制数据包
typedef struct {
  int leftX;
  int leftY;
  int rightX;
  int rightY;
} ControlData;

ControlData controlData;

// ESPNOW 回传数据包
typedef struct Response {
  int responseValue;
} Response;

Response replyData;

// 显示摇杆数据
void displayData() {
  Serial.print("L-X:");
  Serial.println(controlData.leftX);
  Serial.print(" L-Y:");
  Serial.println(controlData.leftY);

  Serial.print("R-X:");
  Serial.println(controlData.rightX);
  Serial.print(" R-Y:");
  Serial.println(controlData.rightY);
}

//电压测量函数
void DisplayAdc() {
  // 读取ADC原始值（取10次平均）
  int adcValue = 0;
  for(int i=0; i<10; i++) {
    adcValue += analogRead(ADC_PIN);
    delay(2);
  }
  adcValue /= 10;

  // 计算实际电压百分比
  voltage = ((adcValue * REF_VOLTAGE / 4095.0) * ((R1 + R2)/R2))*10;
  if (voltage < 56){
    replyData.responseValue = 0;
  }
  else if(voltage > 82){
    replyData.responseValue = 100;
  }
  else {
    replyData.responseValue = map(voltage, 56, 82, 0, 100);
  }
  delay(10);
}

// 发送回调
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Last Packet Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
}

// 接收回调
void OnDataRecv(const esp_now_recv_info_t *recvInfo, const uint8_t *data, int len) {
  char msg[len + 1];
  memcpy(msg, data, len);
  memcpy(&controlData, data, len);
  msg[len] = '\0';

  if (strcmp(msg, PAIRING_REQUEST) == 0) {
    Serial.println("Pairing Request Received");
    memcpy(masterMac, recvInfo->src_addr, 6); 
    
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, masterMac, 6);
    peerInfo.channel = 1;  
    peerInfo.encrypt = false;
    
    if (esp_now_add_peer(&peerInfo) == ESP_OK) {
      Serial.println("Master Peer Added");
      esp_now_send(masterMac, (uint8_t *)PAIRING_RESPONSE, strlen(PAIRING_RESPONSE));
      isPaired = true;
    }
  }
  else {
    //停转
    if ( controlData.leftX == 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, 0);
    }
    if ( controlData.leftY == 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, 0);
    }
    if ( controlData.rightX == 0) {
        analogWrite(motorT1, 0);
        analogWrite(motorT2, 0);
    }
    if ( controlData.rightY == 0) {
        analogWrite(motorP1, 0);
        analogWrite(motorP2, 0);
    }
    //坦克前进
    if (controlData.leftY > 0 && controlData.leftX == 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, controlData.leftY/ratioRL);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, controlData.leftY/ratioRL);
    }

    //坦克后退
    if (controlData.leftY < 0 && controlData.leftX == 0) {
        analogWrite(motorL1, -controlData.leftY/ratioRL);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, -controlData.leftY/ratioRL);
        analogWrite(motorR2, 0);
    }

    //原地左转
    if (controlData.leftX < 0 && controlData.leftY == 0) {
        analogWrite(motorL1, -controlData.leftX/ratioRL);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, -controlData.leftX/ratioRL);
    }

    //原地右转
    if (controlData.leftX > 0 && controlData.leftY == 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, controlData.leftX/ratioRL);
        analogWrite(motorR1, controlData.leftX/ratioRL);
        analogWrite(motorR2, 0);
    }

    //前进左转
    if (controlData.leftX < 0 && controlData.leftY > 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, -controlData.leftX/ratioRL);
    }

    //前进右转
    if (controlData.leftX > 0 && controlData.leftY > 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, controlData.leftX/ratioRL);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, 0);
    }

    //后退左转
    if (controlData.leftX < 0 && controlData.leftY < 0) {
        analogWrite(motorL1, 0);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, -controlData.leftX/ratioRL);
        analogWrite(motorR2, 0);
    }

    //后退右转
    if (controlData.leftX > 0 && controlData.leftY < 0) {
        analogWrite(motorL1, controlData.leftX/ratioRL);
        analogWrite(motorL2, 0);
        analogWrite(motorR1, 0);
        analogWrite(motorR2, 0);
    }

    //炮台左转
    if (controlData.rightX > 0) {
        analogWrite(motorT1, controlData.rightX/ratioT);
        analogWrite(motorT2, 0);
    }

    //炮台右转
    if (controlData.rightX < 0) {
        analogWrite(motorT1, 0);
        analogWrite(motorT2, -controlData.rightX/ratioT);
    }

    //炮管向上
    if (controlData.rightY > 0) {
        analogWrite(motorP1, controlData.rightY/ratioP);
        analogWrite(motorP2, 0);
    }    

    //炮管向下
    if (controlData.rightY < 0) {
        analogWrite(motorP1, 0);
        analogWrite(motorP2, -controlData.rightY/ratioP);
    }

    DisplayAdc();  //读取电压值
    esp_now_send(masterMac, (uint8_t *) &replyData, sizeof(replyData));
    //更新响应时间戳
    lastResponseTime = millis();
  }

}

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(1);

// 初始化LED默认关闭
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW); 

  // 初始化电机引脚
  pinMode(motorL1, OUTPUT);
  pinMode(motorL2, OUTPUT);
  pinMode(motorR1, OUTPUT);
  pinMode(motorR2, OUTPUT);
  pinMode(motorT1, OUTPUT);
  pinMode(motorT2, OUTPUT);
  pinMode(motorP1, OUTPUT);
  pinMode(motorP2, OUTPUT);

  DisplayAdc();   //初始测量电压

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW Init Failed");
    return;
  }
  
  esp_now_register_send_cb(OnDataSent);  // 注册发送回调函数
  esp_now_register_recv_cb(OnDataRecv);  // 注册接收回调函数
}

void loop() {
  //读取当前时间
  unsigned long currentMillis = millis();
  // 根据当前状态选择时间阈值
  long targetInterval = isLedOn ? onInterval : offInterval;
  //显示连接状态控制指示灯
  if (isPaired) {
//    Serial.println("已连接");
    digitalWrite(LED_PIN, HIGH);  // 连接时LED常亮
  } else {
//    Serial.println("未连接");
    if (currentMillis - previousMillis >= targetInterval) {
      previousMillis = currentMillis; // 重置计时器
      isLedOn = !isLedOn;             // 反转状态
      digitalWrite(LED_PIN, isLedOn ? HIGH : LOW); // 更新LED
    }
  }
  delay(10);
  if (millis() - lastResponseTime > RESPONSE_TIMEOUT) {
    esp_now_del_peer(masterMac); // 清除主机地址
    esp_now_register_send_cb(OnDataSent);  // 注册发送回调函数
    esp_now_register_recv_cb(OnDataRecv);  // 注册接收回调函数
    isPaired = false;  //状态未连接
  } 
}
